Generally speaking, a goal of localization and mapping is to compute the most probable location of a trackee using prior sensor and control values (if available). Simultaneous Localization And Mapping (“SLAM”) is a particular localization and mapping technique that uses allothetic sensors, such as image sensors that provide external information about the environment, and idiothetic sensors, which provide information related to the subject's motion in body reference frame, to construct a geometric or topological model of the environment and uses the model for navigation.
Allothetic information may also be used to aid in construction of metric and feature maps. For example, cues such as image features, sonar time-of-flight, color, and/or other external information about the environment may be used to directly recognize a place or situation. Allothetic information may also be used to derive subject motion from measurements of the environment by converting information expressed in the space related to the idiothetic data based on metric models of the associated sensors. With such a metric model, it is possible to infer the relative positions of two places in which allothetic information has been gathered. For example, frame-to-frame stereo camera feature tracking can be used to solve for 6-Degrees Of Freedom (“DOF”) motion of the camera.
Idiothetic information may include speed, acceleration, wheel rotation for vehicles, and/or other information that provides information related to the subject's motion in body reference frame. Dead reckoning operations may use idiothetic information to provide position estimates of the subject in a metric space. Inertial and pressure sensors, which are typically viewed as idiothetic sensors, have been used to provide local allothetic map reference data by inferring the location of terrain features based on how the subject moves through the environment. Inertial and pressure sensors have been effectively used to locate features in structured environments such as stairways and elevators in buildings, which may be used for path corrections.
The limitations and advantages of allothetic and idiothetic sources of information are complementary. One problem associated with the derived metric motion information is that, because it involves a dead reckoning process, it is subject to cumulative error. This may lead to a continuous decrease in quality; therefore, such information may not be trusted over long periods of time. On the other hand, the quality of feature based map information is stationary over time, but suffers from the perceptual aliasing problem. For example, in a given sensor system, two distinct landmarks such as doors or light fixtures in the environment may appear to be the same landmark.
In order to build reliable maps and to navigate for long periods of time, the user track and map information may be combined. In other words, map information may be used to compensate for information drift while user motion/track information may allow perceptually aliased allothetic information to be disambiguated. Techniques have been developed to integrate both allothetic and idiothetic sources of information into a representation useful for navigation. Conventionally, the corresponding representations are referred to as metric maps or topological maps.
In a conventional SLAM problem, the trajectory of the observer—positions, velocities, and headings (etc.) —together with estimates of the map are estimated online without a priori knowledge of a location. Typically, however, navigation and mapping systems may have access to pre-existing map data, which can include, for example, satellite or other aerial imagery, Geographic Information Systems (“GIS”) shape files (including building outlines, roads), elevation maps, and building maps (Computer Assisted Design (“CAD”) files, floor plans, etc.). What is needed is an ability to integrate this existing map information to provide a priori knowledge of a location for navigation and/or modification or extension of SLAM algorithms as new features are discovered.
The SLAM problem has been formulated and solved as a theoretical problem in a number of different forms. However, issues remain in realizing general SLAM solutions in practice and notably in building and using perceptually rich maps as part of a SLAM algorithm. The most common representation is in the form of a state-space model with additive Gaussian noise, leading to the use of the extended Kalman filter (EKF) to solve the SLAM problem.
The EKF provides a recursive solution to the navigation problem and a way to compute consistent estimates for the uncertainty in subject and map landmark locations. This is despite the fact that many sensor noise models are not well represented by additive Gaussian noise.
An important alternative to Kalman Filtering methods is the use of particle filters. Particle filters are a class of nonlinear filters that impose no restriction on the system model, measurement model or nature of the noise statistics. Particle filters compute a solution based on sequential Monte Carlo simulations of particles that are selected to represent the posterior distributions. Particle filters are optimal if given infinite computational resources, but even with limited resources, they can give a better solution than the EKF in cases where the operational region is highly nonlinear. FastSLAM, with its basis in recursive Monte Carlo sampling, or particle filtering, was the first method to directly represent the nonlinear process model and non-Gaussian pose distribution.
Recently, cell phone manufacturers have added navigation sensors to complement Global Positioning System (“GPS”) receivers. These sensors, including triaxial accelerometers, triaxial gyroscopes, triaxial magnetic sensors, barometric pressure sensors, and/or other sensors may be used to track a subject indoors and at other GPS denied environments. Although conventional cell phone applications include the ability to enhance location using cell carrier location services (such as cell tower triangulation) and Wi-Fi (for example, provided by SKYHOOK and now APPLE, GOOGLE, and others), the location accuracy provided by these applications is not adequate. Thus, what is further needed is an ability to leverage the data provided from the embedded sensors to improve location accuracy for subjects in GPS denied or degraded areas, or other areas in which conventional location techniques are deficient.
However, the data provided by the embedded sensors are typically not the same quality as traditional navigation sensors. For example, Micro-Electrical-Mechanical System (“MEMS”) sensors are subject to large inertial drift and other errors that should be accounted for in the design and operation of MEMS-based navigation algorithms.
As such, what is further needed is an ability to perform error reduction to allow reasonable duration of tracking in GPS denied or degraded areas or other areas in which conventional location techniques are deficient. These and other drawbacks exist.